
#include "Mutex.h"
#include "Irq.h"
#include "utility/Types.h"


void InitMutex(MutexType* mutex)
{
    uint32_t eflags = DisableInterrupts();
    mutex->lockedCount=0;
    InitList(&mutex->waitList);
    mutex->owner = (TaskType*)0;
    EnableInterrupts(eflags);
}

void LockMutex(MutexType* mutex)
{
    uint32_t eflags = DisableInterrupts();
    TaskType* current = GetCurrentTask();

    // If not occupied
    if (mutex->lockedCount == 0)
    {
        mutex->lockedCount = 1;
        mutex->owner = current;
    }
    // Occupied
    else if(mutex->owner == current)
    {
        ++mutex->lockedCount;
    }
    // Occupied
    else
    {
        SetTaskBlock(current);
        InsertNodeAtEnd(&mutex->waitList, &current->waitNode);
        DispatchTask();
    } 
    EnableInterrupts(eflags);
}

void UnlockMutex(MutexType* mutex)
{
    uint32_t eflags = DisableInterrupts();
    TaskType* current = GetCurrentTask();
    if (mutex->owner == current)
    {
        if (--mutex->lockedCount == 0)
        {
            mutex->owner = (TaskType*)0;
            if (GetListCount(&mutex->waitList)>0)
            {
                ListNodeType* node = RemoveFirstNode(&mutex->waitList);
                TaskType* task = NodeParent(node, TaskType, waitNode);
                SetTaskReady(task);
                mutex->lockedCount = 1;
                mutex->owner = task;
                DispatchTask();
            }
        }
    }

    EnableInterrupts(eflags);
}